#include "GP3.h"
#include "dialogs/GP3Dialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/surface/gp3.h>
#include <pcl/kdtree/flann.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/ply_io.h>

GP3::GP3()
	: BaseFilter(FilterDescription("Greedy Projection Triangulation", "Greedy Projection Triangulation", "Assembles a local 3D grid over a given pointcloud and downsamples + filters the data.", ":/toolbar/PclUtils/icons/gp3.png", true))
	, m_dialog(0)
	, searchRadius(0)
	, numberOfNeighbours(0)
	, mu(0)
	,maxSurfaceAngle(0)
	,minAngle(0)
	,maxAngle(0)
	,normalConsitency(false)

{

}

GP3::~GP3()
{
	if (m_dialog)
		delete m_dialog;
}

int GP3::openDialog()
{
	if (!m_dialog)
	{
		m_dialog = new GP3Dialog;
	}

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	//if (cloud)
	//{
	//	ccBBox bBox = cloud->getBB(true,false);
	//	if (bBox.isValid())
	//		m_dialog->radiusDoubleSpinBox->setValue(bBox.getDiagNorm() * 0.005);
	//}

	return m_dialog->exec() ? 1 : 0;
}

void GP3::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;

    //fill in parameters from dialog
	searchRadius = m_dialog->SpinBox_searchRadius->value();
	numberOfNeighbours = m_dialog->spinBox_maxNeighbours->value();
	mu = m_dialog->doubleSpinBox_mu->value();
	normalConsitency = m_dialog->checkBox_normalConsistency->checkState();

	maxSurfaceAngle = m_dialog->spinBox_maxSurfaceAngle->value() * M_PI / 180;
	minAngle=m_dialog->spinBox_minAngle->value() * M_PI / 180;
	maxAngle=m_dialog->spinBox_maxAngle->value() * M_PI / 180;
}

int GP3::compute()
{

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	if(!cloud)
		return -1;

	if(!cloud->hasNormals())
		return -14;

	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);

	//get pointcloud in sensor_msgs format
    cc2smReader converter;
    converter.setInputCloud(cloud);
	converter.getAsSM(*sm_cloud);
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	pcl::fromROSMsg(*sm_cloud,*pcl_cloud);

	//Create search Tree
	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
	tree->setInputCloud(pcl_cloud);

	pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
	pcl::PolygonMesh triangles;

	gp3.setSearchRadius(searchRadius);
	gp3.setMaximumNearestNeighbors(numberOfNeighbours);
	gp3.setMu(mu);
	gp3.setMaximumSurfaceAngle(maxSurfaceAngle);
	gp3.setMinimumAngle(minAngle);
	gp3.setMaximumAngle(maxAngle);
	gp3.setNormalConsistency(normalConsitency);

	gp3.setInputCloud(pcl_cloud);
	gp3.setSearchMethod(tree);
	gp3.reconstruct(triangles);

	pcl::io::savePLYFile("mesh.ply",triangles);


	//sensor_msgs::PointCloud2::Ptr out_sm_cloud (new sensor_msgs::PointCloud2);

	//voxelgrid.filter(*out_sm_cloud);

	//boost::shared_ptr<sensor_msgs::PointCloud2> out_cloud_ptr = boost::make_shared<sensor_msgs::PointCloud2>(*out_sm_cloud);

	//sm2ccReader reader;
	//ccPointCloud* out_c = new ccPointCloud;
	//reader.setInputCloud(out_cloud_ptr);
	//int good = reader.getAsCC(out_c);
	//if(good!=1){
	//	delete out_c;
	//	return -1;
	//}

	//ccHObject* cloud_container = new ccHObject();

	//cloud_container->addChild(out_c);
	//cloud_container->setName(qPrintable("voxelgrid"));

	//emit newEntity(cloud_container);


	return 1;
}
